import time
import serial
import cv2 as cv
import math
import numpy as np
import matplotlib.pyplot as plt
ser = serial.Serial()
ser.baudrate = 115200
ser.port = 'COM3'
ser.open()

def get_data(angle, distance):
    '''将球坐标系转换为直角坐标系'''
    x = distance * math.cos(angle * np.pi / 180)
    y = distance * math.sin(angle * np.pi / 180)
    return int(x), int(y)





# 生成一个700*700的空灰度图像
canvas = np.zeros((700, 1400, 3), np.uint8)

point_size = 1
white = (255, 255, 255)
red = (0, 0, 255)
blue = (255, 0, 0)
yellow = (0, 255, 255)

# 绘制雷达显示器界面的同心圆
cv.ellipse(canvas, (700, 700), (600, 600), 1, 180, 360, white, 2)
cv.ellipse(canvas, (700, 700), (450, 450), 1, 180, 360, white, 1)
cv.ellipse(canvas, (700, 700), (300, 300), 1, 180, 360, white, 2)
cv.ellipse(canvas, (700, 700), (100, 100), 1, 180, 360, white, 2)
# 绘制十字线
cv.line(canvas, (700, 100), (700, 700), white, 2)
cv.line(canvas, (0, 700), (1400, 700), white, 3)
# 绘制45°，135°线
start_point = (int(700+600*np.cos(0.25*np.pi)), int(700-600*np.sin(0.25*np.pi)))
end_point = (700, 700)
cv.line(canvas, start_point, end_point, white, 1)
start_point = (int(700-600*np.cos(0.25*np.pi)), int(700-600*np.sin(0.25*np.pi)))
end_point = (700, 700)
cv.line(canvas, start_point, end_point, white, 1)
# 添加正北指向和距离刻度文字
font = cv.FONT_HERSHEY_SIMPLEX
cv.putText(canvas, "Front", (650, 40), font, 1, yellow, 2)
cv.putText(canvas, "180", (10, 680), font, 1, yellow, 2)
cv.putText(canvas, "0", (1300, 680), font, 1, yellow, 2)
# 添加参数指示文字
cv.putText(canvas, "Distance:", (1045, 15), font, 0.5, (255, 255, 255), 1)
cv.putText(canvas, "Angle:", (1065, 35), font, 0.5, (255, 255, 255), 1)
cv.putText(canvas, "Coordinate(X):", (1000, 55), font, 0.5, (255, 255, 255), 1)
cv.putText(canvas, "Coordinate(Y):", (1000, 75), font, 0.5, (255, 255, 255), 1)

# 定义绘制扫描辉亮函数，ang为扫描线所在角度位置
def drawScanner(ang, direction):
    img = np.zeros((700, 1400, 3), np.uint8)
    a = 255 / 60  # 将颜色值255等分60，60为辉亮夹角
    for i in range(60):
        if direction == 1:
            # 逐次绘制1度扇形，颜色从255到0
            cv.ellipse(img, (700, 700), (600, 600), 1, 360 - ang - i , 360 - ang - i - 1, (255 - i * a, 0, 0), -1)
            if 360 - ang - i == 180:
                break
        elif direction == -1:
            cv.ellipse(img, (700, 700), (600, 600), 1, 360 - ang + i - 1, 360 - ang + i, (255 - i * a, 0, 0), -1)
            if 360 - ang - i == 360:
                break
    return img

def mean_blur(data, current_angle, direction):
    new_x = 0
    new_y = 0
    if current_angle > 175 or current_angle < 5:
        new_x, new_y = data[current_angle]
    else:
        if len(data[current_angle - direction * 3]) > 0:
            for i in range(3):
                new_x += data[current_angle - i * direction][0]
                new_y += data[current_angle - i * direction][1]
        new_x = new_x // 3
        new_y = new_y // 3

    return [new_x, new_y]




delteT = 12  # 目标运动的比例值
temp_angle = 181  # 初始角度，后面用来判别方向
direction = 1   # 1为顺时针，-1 为逆时针
data = [[] for i in range(181)]
after_blur_data = [[] for j in range(181)]
a = 0
while(ser.is_open):
    s = ser.readline()
    s = str(s, encoding="utf-8")

    angle = int(s.split("Distance:")[0].split("Angle:")[-1])
    distance = int(s.split("Distance:")[-1].split(r"\r\n")[0])

    point_x, point_y = get_data(angle, distance)
    if distance > 50:
        point_x, point_y = 0, 0
    temp = np.copy(canvas)

    data[180 - angle] = [point_x, point_y]
    after_blur_data[180 - angle] = mean_blur(data, 180 - angle, direction)

    for i in range(len(after_blur_data)):
        if len(after_blur_data[i]) > 0:
                cv.circle(temp, (700 + after_blur_data[i][0] * delteT, 700 - after_blur_data[i][1] * delteT), point_size, red, 3)  # 目标点o


    # 复制雷达界面，将目标运动和参数指示绘制在复制图上

    cv.putText(temp, str(distance), (1120, 15), font, 0.5, (0, 255, 0), 1)
    cv.putText(temp, str(angle), (1120, 35), font, 0.5, (0, 255, 0), 1)
    cv.putText(temp, str(point_x), (1120, 55), font, 0.5, (0, 255, 0), 1)
    cv.putText(temp, str(point_y), (1120, 75), font, 0.5, (0, 255, 0), 1)

    scanImg = drawScanner(angle, direction)  # 绘制扫描辉亮
    blend = cv.addWeighted(temp, 1.0, scanImg, 0.6, 0.0)  # 将雷达显示与扫描辉亮混合

    cv.imshow('My Radar', blend)
    if angle - temp_angle < 0:
        direction = 1
    else:
        direction = -1
    a += 1
    # if (a // 180) >= 4:
    #     plt.plot([i for i in range(180)], [math.sqrt(data[i][0]**2 + data[i][1]**2) for i in range(180)], label="Raw Data")
    #     plt.plot([i for i in range(180)], [math.sqrt(after_blur_data[i][0] ** 2 + after_blur_data[i][1] ** 2) for i in range(180)],
    #              label="After Mean Blur Data")
    #     plt.xlabel("Angle")
    #     plt.ylabel("Distance")
    #     plt.title("Error Analyze")
    #     plt.legend()
    #     plt.show()

    temp_angle = angle
    cv.waitKey(1)